FUNCTION_BLOCK LCB_9400Drive
TITLE = 'LCB_8400Drive'


// This FB read and write 15 Words of processdata from a connected 9400 Lenze Drive
// The implementation is independant of the used bussystem (Profinet / Profibus)
//
// Used S7 Systemfunctions 
// SFC14 (DPRD_DAT)
// SFC15 (DPWR_DAT)
//
// Versionshistory:
// 1.0 Ersterstellung E.Dittmann


AUTHOR : 'Lenze'
Family : 'LCB'
Version : '1.0'
Name: '94Drive'



CONST
    Range := 15; // Count of wors from / to the drive 
END_CONST

VAR_INPUT
    iFirstPeripherieADR         :INT;                               // Peripheral adress of the drive (Hardwareconfig S7)
    xEnableInternalControl      :BOOL:=False;                       // Enable internal drivecontrol with variabletable
    wLPortAxisIn1_wControl      :WORD:=0;
    iLPortAxisIn1_nIn1          :INT:=0;
    diLPortAxisIn1_dnIn2        :DINT:=0;
    wLPortControl1              :WORD:=0;
    wLPortControl2              :WORD:=0;
    diLPort32In1                :DINT:=0;
    diLPort32In2                :DINT:=0;
    diLPort32In3                :DINT:=0;
    iLPort16In1                 :INT:=0;
    iLPort16In2                 :INT:=0;
    iLPort16In3                 :INT:=0;
END_VAR

VAR_OUTPUT
    xError                      :BOOL;                               // Error
    iReadError                  :INT;                                // Errorcode SFC14 
    iWriteError                 :INT;                                // Errorcode SFC15  
    xInternalControlActive      :BOOL;                               // Internal control active
    xCommunicationOK            :BOOL;                               // Communication OK
    wLPortAxisOut1_wStatus      :WORD;
    iLPortAxisOut1_nOut1        :INT;
    diLPortAxisOut1_dnOut2      :DINT;
    wLPortStatus1               :WORD;
    wLPortStatus2               :WORD;
    diLPort32Out1               :DINT;
    diLPort32Out2               :DINT;
    diLPort32Out3               :DINT;
    iLPort16Out1                :INT;
    iLPort16Out2                :INT;
    iLPort16Out3                :INT;   
    
END_VAR

VAR_IN_OUT

END_VAR


VAR
    wFirstPeripherieADR         :WORD;                               
//    awSend                      :ARRAY[1..Range] OF WORD;                         
//    awRecieve                   :ARRAY[1..Range] OF WORD;
    scInternalCtrl:             STRUCT                                  
                                    b8:     BOOL:= FALSE;
                                    b9:     BOOL:= FALSE;
                                    b10:    BOOL:= FALSE;
                                    b11:    BOOL:= FALSE;
                                    b12:    BOOL:= FALSE;
                                    b13:    BOOL:= FALSE;
                                    b14:    BOOL:= FALSE;
                                    b15:    BOOL:= FALSE;
                                    b0:     BOOL:= FALSE;
                                    b1:     BOOL:= FALSE;
                                    b2:     BOOL:= FALSE;
                                    b3:     BOOL:= FALSE;
                                    b4:     BOOL:= FALSE;
                                    b5:     BOOL:= FALSE;
                                    b6:     BOOL:= FALSE;
                                    b7:     BOOL:= FALSE;
                                    awInt:  ARRAY[2..Range] OF WORD;
                                END_STRUCT;
                            
    scInternalState:            STRUCT                                  
                                    awInt:  ARRAY[1..Range] OF WORD;
                                END_STRUCT;

    wCTRL_IC                    :WORD:=0;
    xwCtrl_IC AT wCTRL_IC       :ARRAY[0..15] OF BOOL;
    iLPortAxisIn1_nIn1_IC       :INT:=0;
    diLPortAxisIn1_dnIn2_IC     :DINT:=0;
    wLPortControl1_IC           :WORD:=0;
    wLPortControl2_IC           :WORD:=0;
    diLPort32In1_IC             :DINT:=0;
    diLPort32In2_IC             :DINT:=0;
    diLPort32In3_IC             :DINT:=0;
    iLPort16In1_IC              :INT:=0;
    iLPort16In2_IC              :INT:=0;
    iLPort16In3_IC              :INT:=0;
    
    


  END_VAR

BEGIN

wFirstPeripherieADR:= INT_TO_WORD(iFirstPeripherieADR);             
iReadError:=0;
iWriteerror:=0;



IF xEnableInternalControl THEN                                    // Interne Steuerung des Reglers
   xInternalControlActive:=True;    
                        
ELSE 
   xInternalControlActive:=False;                                   // Steuerung des Reglers über Eingangsvariablen des Bausteins
   wCtrl_IC   := wLPortAxisIn1_wControl;
   scInternalCtrl.b8 := xwCtrl_IC[8];
   scInternalCtrl.b9 := xwCtrl_IC[9];
   scInternalCtrl.b10 := xwCtrl_IC[10];
   scInternalCtrl.b11 := xwCtrl_IC[11];
   scInternalCtrl.b12 := xwCtrl_IC[12];
   scInternalCtrl.b13 := xwCtrl_IC[13];
   scInternalCtrl.b14 := xwCtrl_IC[14];
   scInternalCtrl.b15 := xwCtrl_IC[15];
   scInternalCtrl.b0 := xwCtrl_IC[0];
   scInternalCtrl.b1 := xwCtrl_IC[1];
   scInternalCtrl.b2 := xwCtrl_IC[2];
   scInternalCtrl.b3 := xwCtrl_IC[3];
   scInternalCtrl.b4 := xwCtrl_IC[4];
   scInternalCtrl.b5 := xwCtrl_IC[5];
   scInternalCtrl.b6 := xwCtrl_IC[6];
   scInternalCtrl.b7 := xwCtrl_IC[7];
   iLPortAxisIn1_nIn1_IC := iLPortAxisIn1_nIn1;    
   diLPortAxisIn1_dnIn2_IC := diLPortAxisIn1_dnIn2;    
   wLPortControl1_IC := wLPortControl1;        
   wLPortControl2_IC := wLPortControl2;        
   diLPort32In1_IC := diLPort32In1;            
   diLPort32In2_IC := diLPort32In2;             
   diLPort32In3_IC := diLPort32In3;        
   iLPort16In1_IC  := iLPort16In1;            
   iLPort16In2_IC  := iLPort16In2;            
   iLPort16In3_IC  := iLPort16In3;      
END_IF;
           
iReadError:= DPRD_DAT(
                        LADDR := wFirstPeripherieADR, 
                        RECORD := scInternalState ); 
                        
// Prozessdaten auf den Ausgängen aktualisieren
    wLPortAxisOut1_wStatus  := scInternalState.awInt[1];
    iLPortAxisOut1_nOut1    := WORD_TO_INT(scInternalState.awInt[2]);
    diLPortAxisOut1_dnOut2  := WORD_TO_DINT(scInternalState.awInt[4]) + DWORD_TO_DINT(SHL(IN:=WORD_TO_DWORD(scInternalState.awInt[3]),N:=16));
    wLPortStatus1           := scInternalState.awInt[5];
    wLPortStatus2           := scInternalState.awInt[6];
    diLPort32Out1           := WORD_TO_DINT(scInternalState.awInt[8]) + DWORD_TO_DINT(SHL(IN:=WORD_TO_DWORD(scInternalState.awInt[7]),N:=16));
    diLPort32Out2           := WORD_TO_DINT(scInternalState.awInt[10]) + DWORD_TO_DINT(SHL(IN:=WORD_TO_DWORD(scInternalState.awInt[9]),N:=16));
    diLPort32Out3           := WORD_TO_DINT(scInternalState.awInt[12]) + DWORD_TO_DINT(SHL(IN:=WORD_TO_DWORD(scInternalState.awInt[11]),N:=16));
    iLPort16Out1            := WORD_TO_INT(scInternalState.awInt[13]);
    iLPort16Out2            := WORD_TO_INT(scInternalState.awInt[14]);
    iLPort16Out3            := WORD_TO_INT(scInternalState.awInt[15]);    
    
// Schreiben der Eingänge auf die Prozessdaten
    scInternalCtrl.awInt[2]               := INT_TO_WORD(iLPortAxisIn1_nIn1_IC);
    scInternalCtrl.awInt[4]               := DINT_TO_WORD(diLPortAxisIn1_dnIn2_IC);
    scInternalCtrl.awInt[3]               := DWORD_TO_WORD(SHR(IN:=DINT_TO_DWORD(diLPortAxisIn1_dnIn2_IC),N:=16));
    scInternalCtrl.awInt[5]               := wLPortControl1_IC;
    scInternalCtrl.awInt[6]               := wLPortControl2_IC;
    scInternalCtrl.awInt[8]               := DINT_TO_WORD(diLPort32In1_IC);
    scInternalCtrl.awInt[7]               := DWORD_TO_WORD(SHR(IN:=DINT_TO_DWORD(diLPort32In1_IC),N:=16));
    scInternalCtrl.awInt[10]              := DINT_TO_WORD(diLPort32In2_IC);
    scInternalCtrl.awInt[9]               := DWORD_TO_WORD(SHR(IN:=DINT_TO_DWORD(diLPort32In2_IC),N:=16));
    scInternalCtrl.awInt[12]              := DINT_TO_WORD(diLPort32In3_IC);
    scInternalCtrl.awInt[11]              := DWORD_TO_WORD(SHR(IN:=DINT_TO_DWORD(diLPort32In3_IC),N:=16));
    scInternalCtrl.awInt[13]              := INT_TO_WORD(iLPort16In1_IC);
    scInternalCtrl.awInt[14]              := INT_TO_WORD(iLPort16In2_IC);
    scInternalCtrl.awInt[15]              := INT_TO_WORD(iLPort16In3_IC);
   
   
                        
iWriteError:=DPWR_DAT(
                        LADDR := wFirstPeripherieADR, 
                        RECORD := scInternalCtrl ); 
                        
                                                                    // Setzen der Stausvariablen
IF (iReadError <> 0) OR (iWriteError <> 0) THEN
    xError:= true;
    xCommunicationOK:=false;
ELSE
    xError:= false;
    xCommunicationOK:=True;
END_IF;
     
END_FUNCTION_BLOCK